#include "./flash.h"
#include "../xlib/compat.h"
#include <string.h>

namespace driver::flash {

using namespace utils;

namespace mem {

template <typename T, typename A> inline T read(A addr) {
    return *reinterpret_cast<volatile T *>(addr);
}

template <typename T, typename A> inline void write(A addr, T value) {
    *reinterpret_cast<volatile T *>(addr) = value;
}

} // namespace mem

constexpr uint32_t FLASH_KEY1 = 0x45670123;
constexpr uint32_t FLASH_KEY2 = 0xCDEF89AB;

static void unlock() {
    if (FLASH->CR & FLASH_CR_LOCK) {
        FLASH->KEYR = FLASH_KEY1;
        FLASH->KEYR = FLASH_KEY2;
    }
}

static Error wait_done() {

    for (;;) {
        uint32_t tmp = FLASH->SR;

        if (tmp & FLASH_SR_BSY) {
            continue;
        } else if (tmp & (FLASH_SR_PGERR | FLASH_SR_WRPRTERR)) {
            return ec::FLASH_ERR;
        } else {
            return ec::OK;
        }
    }
}

Error write(uint32_t addr, const void *buf, uint32_t size) {
    const uint16_t *src = reinterpret_cast<const uint16_t *>(buf);
    size = (size + 1) / 2;

    Error err = wait_done();
    if (err) {
        goto finish;
    }

    unlock();
    FLASH->CR |= FLASH_CR_PG;

    for (uint32_t i = 0; i < size; i++) {
        mem::write(addr, src[i]);

        err = wait_done();
        if (err) {
            goto finish;
        }

        if (mem::read<uint16_t>(addr) != src[i]) {
            err = ec::FLASH_ERR;
            goto finish;
        }

        addr += 2;
    }
finish:
    FLASH->CR &= ~FLASH_CR_PG;
    return err;
}

static Error erase(uint32_t addr) {
    Error err = wait_done();
    if (err) {
        goto finish;
    }
    FLASH->CR |= FLASH_CR_PER;
    FLASH->AR = addr;
    FLASH->CR |= FLASH_CR_STRT;
    err = wait_done();
    FLASH->CR &= ~FLASH_CR_PER;
    if (err) {
        goto finish;
    }
finish:
    return err;
}

Error erase(uint32_t addr, uint32_t size) {
    uint32_t index = 0;
    Error err;
    unlock();
    for (; index < size;) {
        err = erase(addr + index);
        if (err) {
            goto finish;
        }
        index += SECTOR_SIZE;
    }
finish:
    return err;
}

void read(uint32_t addr, void *buf, uint32_t size) {
    memcpy(buf, reinterpret_cast<const void *>(addr), size);
}

} // namespace driver::flash
